#include "CGM.h"

using namespace Method;

CGM::CGM(shared_ptr<const Matrix> SLAEMatrix, shared_ptr<const Matrix> Preconditioner,
		shared_ptr<const Vector> RightPart, shared_ptr<const Vector> Approximation)
{
	_matrix = SLAEMatrix;
	_preconditioner = Preconditioner;
	_rightPart = RightPart;
	_approximation = Approximation;


	r1.resize(RightPart->size());
	Az.resize(RightPart->size());
	z1.resize(RightPart->size());
	_temp_vector.resize(RightPart->size());
	p.resize(RightPart->size());


	if (Approximation != nullptr)
		_result = shared_ptr<Vector>(new Vector(*Approximation));
	else _result = shared_ptr<Vector>(new Vector(_rightPart->size()));

	_matrix->MultMatrixVector(*_result, _temp_vector);
	r1 = *_rightPart - _temp_vector;

	_preconditioner->multLInvToVector(true, r1, _temp_vector);
	_preconditioner->multUInvToVector(true, _temp_vector, z1);
	p = z1; //M^-1R
	_residual = 0;
}

CGM::~CGM()
{
}

void CGM::nextIteration() 
{
	_matrix->MultMatrixVector(z1, Az);
	double MRR = p * r1;
	alpha = MRR/ (z1 * Az);
	*_result += z1 * alpha;
	r1 -= Az * alpha;

	_matrix->multLInvToVector(true, r1, _temp_vector);
	_matrix->multUInvToVector(true, _temp_vector, p);

	beta = p * r1 / MRR;
	z1 = p+z1*beta;

	_residual = sqrt(r1 * r1);
}
